package org.jdesktop.wonderland.modules.kinectcontrol.client;

import java.util.ArrayList;
import java.util.Collections;
import java.util.Iterator;

/**
 * Dynamic Time Warping nearest neighbour sequence comparison class. Called
 * 'Gesture Recognizer' but really it can work with any vectors
 *
 * (C) By Rhemyst. Dude's a freakin' genius. Also he can do the Rubik's Cube. I
 * mean REALLY do the Rubik's Cube. Copied to Java by Dominik Alessandri HSLU
 * T&A
 *
 * @author Dominik Alessandri
 */
public class DTWDetectionRecognizer {

    /**
     * Size of obeservations vectors.
     */
    private final int _dimension;
    /**
     * Maximum distance between the last observations of each sequence.
     */
    private final double _firstThreshold;
    /**
     * Minimum length of a gesture before it can be recognised
     */
    private final double _minimumLength;
    /**
     * Maximum DTW distance between an example and a sequence being classified.
     */
    private final double _globalThreshold;
    /**
     * The gesture names. Index matches that of the sequences array in
     * _sequences
     */
    private final ArrayList<String> _labels;
    /**
     * Maximum vertical or horizontal steps in a row.
     */
    private final int _maxSlope;
    /**
     * The recorded gesture sequences
     */
    private final ArrayList<ArrayList<double[]>> _sequences;

    /**
     * Initializes a new instance of the DtwGestureRecognizer class First DTW
     * constructor
     *
     * @param dim Vector
     * @param threshold Maximum distance between the last observations of each
     * sequence
     * @param firstThreshold Minimum threshold
     * @param minLen
     */
    public DTWDetectionRecognizer(int dim, double threshold, double firstThreshold, double minLen) {
        _dimension = dim;
        _sequences = new ArrayList<ArrayList<double[]>>();
        _labels = new ArrayList<String>();
        _globalThreshold = threshold;
        _firstThreshold = firstThreshold;
        _maxSlope = Integer.MAX_VALUE;
        _minimumLength = minLen;
    }

    /**
     * Initializes a new instance of the DtwGestureRecognizer class Second DTW
     * constructor
     *
     * @param dim Vector
     * @param threshold Maximum distance between the last observations of each
     * sequence
     * @param firstThreshold Minimum threshold
     * @param ms Maximum vertical or horizontal steps in a row
     * @param minLen
     */
    public DTWDetectionRecognizer(int dim, double threshold, double firstThreshold, int ms, double minLen) {
        _dimension = dim;
        _sequences = new ArrayList<ArrayList<double[]>>();
        _labels = new ArrayList<String>();
        _globalThreshold = threshold;
        _firstThreshold = firstThreshold;
        _maxSlope = ms;
        _minimumLength = minLen;
    }

    /**
     * Add a seqence with a label to the known sequences library. The gesture
     * MUST start on the first observation of the sequence and end on the last
     * one. Sequences may have different lengths.
     *
     * @param seq The sequence
     * @param lab Sequence name
     */
    public void AddOrUpdate(ArrayList<double[]> seq, String lab) {
        // First we check whether there is already a recording for this label. If so overwrite it, otherwise add a new entry
        int existingIndex = -1;

        for (int i = 0; i < _labels.size(); i++) {
            if (_labels.get(i).equals(lab)) {
                existingIndex = i;
            }
        }

        // If we have a match then remove the entries at the existing index to avoid duplicates. We will add the new entries later anyway
        if (existingIndex >= 0) {
            _sequences.remove(existingIndex);
            _labels.remove(existingIndex);
        }

        // Add the new entries
        _sequences.add(seq);
        _labels.add(lab);
    }

    /**
     * Recognize gesture in the given sequence. It will always assume that the
     * gesture ends on the last observation of that sequence. If the distance
     * between the last observations of each sequence is too great, or if the
     * overall DTW distance between the two sequence is too great, no gesture
     * will be recognized.
     *
     * @param seq The sequence to recognise
     * @return The recognised gesture name
     */
    public String Recognize(ArrayList<double[]> seq) {

        double minDist = Double.POSITIVE_INFINITY;
        String classification = "__UNKNOWN";

        for (int i = 0; i < _sequences.size(); i++) {

            ArrayList<double[]> example = _sequences.get(i);

            ////Debug.WriteLine(Dist2((double[]) seq[seq.Count - 1], (double[]) example[example.Count - 1]));
            if (Dist2(seq.get(seq.size() - 1), example.get(example.size() - 1)) <_firstThreshold){
                
                double d = Dtw(seq, example) / example.size();
                if (d < minDist) {
                    minDist = d;
                    classification = (String) _labels.toArray()[i];
                }
            }
        }

        return (minDist < _globalThreshold ? classification : "__UNKNOWN") + " " /*+minDist.ToString()*/;
    }

    /**
     * Computes a 1-distance between two observations. (aka Manhattan distance).
     *
     * @param a Point a (double)
     * @param b Point b (double)
     * @return Manhattan distance between the two points
     */
    private double Dist1(double[] a, double[] b) {
        double d = 0;
        for (int i = 0; i < _dimension; i++) {
            d += Math.abs(a[i] - b[i]);
        }

        return d;
    }

    /**
     * Computes a 2-distance between two observations. (aka Euclidian distance).
     *
     * @param a Point a (double)
     * @param b Point b (double)
     * @return Euclidian distance between the two points
     */
    private double Dist2(double[] a, double[] b) {
        double d = 0;
        for (int i = 0; i < _dimension; i++) {
            d += Math.pow(a[i] - b[i], 2);
        }

        return Math.sqrt(d);
    }

    /**
     * Retrieves a text represeantation of the _label and its associated _sequence
     * For use in dispaying debug information and for saving to file
     * 
     * @return A string containing all recorded gestures and their names
     */
    public String RetrieveText() {
        
        String retStr = "";

        if (_sequences != null) {
            // Iterate through each gesture
            for (int gestureNum = 0; gestureNum < _sequences.size(); gestureNum++) {
                // Echo the label
                retStr += _labels.toArray()[gestureNum] + "\r\n";

                int frameNum = 0;

                //Iterate through each frame of this gesture
                Iterator<double[]> iterator1 = _sequences.get(gestureNum).iterator();
                while(iterator1.hasNext())
                {
                    double[] frames = iterator1.next();
                    // Extract each double
                    for(int i = 0; i < frames.length; i++)
                    {
                        retStr += frames[i] + "\r\n";
                    }

                    // Signifies end of this double
                    retStr += "~\r\n";

                    frameNum++;
                }

                // Signifies end of this gesture
                retStr += "----";
                if (gestureNum < _sequences.size() - 1) {
                    retStr += "\r\n";
                }
            }
        }

        return retStr;
    }
    
    /**
     * Compute the min DTW distance between seq2 and all possible endings of seq1.
     * 
     * @param seq1 The first array of sequences to compare
     * @param seq2 The second array of sequences to compare
     * @return The best match
     */
        public double Dtw(ArrayList<double[]> seq1, ArrayList<double[]> seq2)
        {
            // Init
            ArrayList<double[]> seq1R = new ArrayList(seq1);
            Collections.reverse(seq1R);
            ArrayList<double[]> seq2R = new ArrayList(seq2);
            Collections.reverse(seq2R);
            double[][] tab = new double[seq1R.size() + 1][seq2R.size() + 1];
            int[][] slopeI = new int[seq1R.size() + 1][seq2R.size() + 1];
            int[][] slopeJ = new int[seq1R.size() + 1][seq2R.size() + 1];

            for (int i = 0; i < seq1R.size() + 1; i++)
            {
                for (int j = 0; j < seq2R.size() + 1; j++)
                {
                    tab[i][j] = Double.POSITIVE_INFINITY;
                    slopeI[i][j] = 0;
                    slopeJ[i][j] = 0;
                }
            }

            tab[0][0] = 0;

            // Dynamic computation of the DTW matrix.
            for (int i = 1; i < seq1R.size() + 1; i++)
            {
                for (int j = 1; j < seq2R.size() + 1; j++)
                {
                    if (tab[i][j-1] < tab[i - 1][j - 1] && tab[i][j - 1] < tab[i - 1][j] && slopeI[i][j - 1] < _maxSlope)
                    {
                        tab[i][j] = Dist2(seq1R.get(i - 1),  seq2R.get(j - 1)) + tab[i][j - 1];
                        slopeI[i][j] = slopeJ[i][j - 1] + 1;
                        slopeJ[i][j] = 0;
                    }
                    else if (tab[i - 1][j] < tab[i - 1][j - 1] && tab[i - 1][j] < tab[i][j - 1] && slopeJ[i - 1][j] < _maxSlope)
                    {
                        tab[i][j] = Dist2(seq1R.get(i - 1),  seq2R.get(j - 1)) + tab[i - 1][j];
                        slopeI[i][j] = 0;
                        slopeJ[i][j] = slopeJ[i - 1][j] + 1;
                    }
                    else
                    {
                        tab[i][j] = Dist2(seq1R.get(i - 1),  seq2R.get(j - 1)) + tab[i - 1][j - 1];
                        slopeI[i][j] = 0;
                        slopeJ[i][j] = 0;
                    }
                }
            }

            // Find best between seq2 and an ending (postfix) of seq1.
            double bestMatch = Double.POSITIVE_INFINITY;
            for (int i = 1; i < (seq1R.size() + 1) - _minimumLength; i++)
            {
                if (tab[i][seq2R.size()] < bestMatch)
                {
                    bestMatch = tab[i][seq2R.size()];
                }
            }

            return bestMatch;
        }
}
